package roboticsfmw.sample.sensor;

import roboticsfmw.annotation.HardwareRef;
import roboticsfmw.core.Sensor;
import roboticsfmw.sample.event.JoystickEvent;
import roboticsfmw.sample.hardware.JoystickHardwareProxy;

import com.centralnexus.input.Joystick;
import com.centralnexus.input.JoystickListener;

public class JoystickSensor extends Sensor<JoystickHardwareProxy> implements JoystickListener{

	private int lastY;
	private int lastX;
	private int lastZ;
	
	@HardwareRef(name="/hw/joystick")
	private JoystickHardwareProxy joystickHardware;

	public JoystickSensor(String name, long pollInterval, boolean loop) throws IllegalArgumentException {
		super(name, pollInterval, loop);
	}
	
	public void setJoystickHardware(JoystickHardwareProxy joystickHardware) {
		this.joystickHardware = joystickHardware;
	}

	@Override
	public void init() {
		joystickHardware.getJoystick().addJoystickListener(this);
	}

	@Override
	public void loop(long i) {
	}
	
	@Override
	public void shutdown() {
		joystickHardware.getJoystick().removeJoystickListener(this);
	}

	@Override
	public void joystickAxisChanged(Joystick joystick) {

		int x = (int) Math.round( 100 * joystick.getX() );
		int y = (int) Math.round( 100 * joystick.getY() );
		int z = (int) Math.round( -50 * (joystick.getZ()-1) );

		JoystickEvent event = new JoystickEvent();
		event.setX(x);
		event.setY(y);
		event.setZ(z);

		if ( x!=this.lastX) {
			this.lastX = x;
			event.setXChanged(true);
		}
		if (y!=this.lastY) {
			this.lastY = y;
			event.setYChanged(true);
		}
		if (z!=this.lastZ) {
			this.lastZ = z;
			event.setZChanged(true);
		}
		if (event.isXChanged() || event.isYChanged() || event.isZChanged()) {
			getEventBus().notify(event);
		}
	}

	@Override
	public void joystickButtonChanged(Joystick joystick) {
		JoystickEvent event = new JoystickEvent();
		if (joystick.isButtonDown(32768)) {
			event.setB1(true);
		} else if (joystick.isButtonDown(131072)){
			event.setB2(true);
		}
		getEventBus().notify(event);
	}

}
